#ifndef CARTESIAN_PATH_NOLOCK_H
#define CARTESIAN_PATH_NOLOCK_H

// CombinedNode.h
#pragma once

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <atomic>
#include <memory>

class CombinedNode : public rclcpp::Node {
public:
    explicit CombinedNode();  // 显式声明构造函数
    
    // 导出执行接口
    void start_execution(int path_index);

private:
    // 内部实现成员
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr force_sub_;
    std::atomic<bool> force_sign;
    std::atomic<size_t> closest_index;

    // 内部方法声明
    void force_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);
    bool returnToMiddle();
    bool continueCartesianPath(const std::vector<geometry_msgs::msg::Pose>& original_waypoints);
    void executeCartesianPath1();
    void executeCartesianPath2();
    void executeCartesianPath3();
};

#endif // CARTESIAN_PATH_NOLOCK_H